{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "0\n"
     ]
    }
   ],
   "source": [
    "# Car's movement control (forward, back, left, right, brake)\n",
    "# motor control\n",
    "# -*- coding:UTF-8 -*-\n",
    "import RPi.GPIO as GPIO\n",
    "import time\n",
    "\n",
    "#   小车电机引脚定义\n",
    "IN1 = 20\n",
    "IN2 = 21\n",
    "IN3 = 19\n",
    "IN4 = 26\n",
    "ENA = 16\n",
    "ENB = 13\n",
    "#RGB三色灯引脚定义\n",
    "LED_R = 22\n",
    "LED_G = 27\n",
    "LED_B = 24\n",
    "#设置RGB三色灯为BCM编码方式\n",
    "GPIO.setmode(GPIO.BCM)\n",
    "# global pwm_ENA\n",
    "# global pwm_ENB\n",
    "global delaytime\n",
    "# 设置GPIO口为BCM编码方式\n",
    "GPIO.setmode(GPIO.BCM)\n",
    "\n",
    "# 忽略警告信息\n",
    "GPIO.setwarnings(False)\n",
    "buzzer = 8  #蜂鸣器引脚开关，具体建议参考硬件参考手册\n",
    "#设置GPIO口为BCM编码方式\n",
    "GPIO.setmode(GPIO.BCM)    \n",
    "GPIO.setup(buzzer,GPIO.OUT)\n",
    "\n",
    "class CarMove(object):\n",
    "    def __init__(self):\n",
    "        GPIO.setup(LED_R, GPIO.OUT)\n",
    "        GPIO.setup(LED_G, GPIO.OUT)\n",
    "        GPIO.setup(LED_B, GPIO.OUT)\n",
    "        GPIO.setup(ENA, GPIO.OUT, initial=GPIO.HIGH)\n",
    "        GPIO.setup(IN1, GPIO.OUT, initial=GPIO.LOW)\n",
    "        GPIO.setup(IN2, GPIO.OUT, initial=GPIO.LOW)\n",
    "        GPIO.setup(ENB, GPIO.OUT, initial=GPIO.HIGH)\n",
    "        GPIO.setup(IN3, GPIO.OUT, initial=GPIO.LOW)\n",
    "        GPIO.setup(IN4, GPIO.OUT, initial=GPIO.LOW)\n",
    "        # 设置pwm引脚和频率为2000hz\n",
    "        self.pwm_ENA = GPIO.PWM(ENA, 2000)\n",
    "        self.pwm_ENB = GPIO.PWM(ENB, 2000)\n",
    "        self.pwm_ENA.start(0)\n",
    "        self.pwm_ENB.start(0)\n",
    "        self.leftSpeed= 10\n",
    "        self.rightSpeed = 10\n",
    "        \n",
    "    #小车前进\t\n",
    "    def run(self,leftSpeed,rightSpeed,delaytime):\n",
    "        GPIO.output(IN1, GPIO.HIGH)\n",
    "        GPIO.output(IN2, GPIO.LOW)\n",
    "        GPIO.output(IN3, GPIO.HIGH)\n",
    "        GPIO.output(IN4, GPIO.LOW)\n",
    "        self.pwm_ENA.ChangeDutyCycle(80)\n",
    "        self.pwm_ENB.ChangeDutyCycle(80)\n",
    "        time.sleep(delaytime)\n",
    "\n",
    "    \n",
    "\n",
    "    def forward(self, leftSpeed, rightSpeed, delaytime):\n",
    "        GPIO.output(IN1, GPIO.HIGH)\n",
    "        GPIO.output(IN2, GPIO.LOW)\n",
    "        GPIO.output(IN3, GPIO.HIGH)\n",
    "        GPIO.output(IN4, GPIO.LOW)\n",
    "        self.pwm_ENA.ChangeDutyCycle(leftSpeed)\n",
    "        self.pwm_ENB.ChangeDutyCycle(rightSpeed)\n",
    "        time.sleep(delaytime)\n",
    "\n",
    "    # 小车后退\n",
    "    def back(self, leftSpeed, rightSpeed, delaytime):\n",
    "        GPIO.output(IN1, GPIO.LOW)\n",
    "        GPIO.output(IN2, GPIO.HIGH)\n",
    "        GPIO.output(IN3, GPIO.LOW)\n",
    "        GPIO.output(IN4, GPIO.HIGH)\n",
    "        self.pwm_ENA.ChangeDutyCycle(leftSpeed)\n",
    "        self.pwm_ENB.ChangeDutyCycle(rightSpeed)\n",
    "        time.sleep(delaytime)\n",
    "\n",
    "    # 小车左转\n",
    "    def left(self, leftSpeed, rightSpeed, delaytime):\n",
    "        GPIO.output(IN1, GPIO.LOW)\n",
    "        GPIO.output(IN2, GPIO.LOW)\n",
    "        GPIO.output(IN3, GPIO.HIGH)\n",
    "        GPIO.output(IN4, GPIO.LOW)\n",
    "        self.pwm_ENA.ChangeDutyCycle(leftSpeed)\n",
    "        self.pwm_ENB.ChangeDutyCycle(rightSpeed)\n",
    "        time.sleep(delaytime)\n",
    "\n",
    "    # 小车右转\n",
    "    def right(self, leftSpeed, rightSpeed, delaytime):\n",
    "        GPIO.output(IN1, GPIO.HIGH)\n",
    "        GPIO.output(IN2, GPIO.LOW)\n",
    "        GPIO.output(IN3, GPIO.LOW)\n",
    "        GPIO.output(IN4, GPIO.LOW)\n",
    "        self.pwm_ENA.ChangeDutyCycle(leftSpeed)\n",
    "        self.pwm_ENB.ChangeDutyCycle(rightSpeed)\n",
    "        time.sleep(delaytime)\n",
    "\n",
    "    '''\n",
    "    小车原地左转\n",
    "    左前轮倒转同速、右后轮前进同速\n",
    "    '''\n",
    "    def spin_left(self, leftSpeed, rightSpeed, delaytime):\n",
    "        GPIO.output(IN1, GPIO.LOW)\n",
    "        GPIO.output(IN2, GPIO.HIGH)\n",
    "        GPIO.output(IN3, GPIO.HIGH)\n",
    "        GPIO.output(IN4, GPIO.LOW)\n",
    "        self.pwm_ENA.ChangeDutyCycle(leftSpeed)\n",
    "        self.pwm_ENB.ChangeDutyCycle(rightSpeed)\n",
    "        time.sleep(delaytime)\n",
    "\n",
    "    # 小车原地右转\n",
    "    def spin_right(self, leftSpeed, rightSpeed, delaytime):\n",
    "        GPIO.output(IN1, GPIO.HIGH)\n",
    "        GPIO.output(IN2, GPIO.LOW)\n",
    "        GPIO.output(IN3, GPIO.LOW)\n",
    "        GPIO.output(IN4, GPIO.HIGH)\n",
    "        self.pwm_ENA.ChangeDutyCycle(leftSpeed)\n",
    "        self.pwm_ENB.ChangeDutyCycle(rightSpeed)\n",
    "        time.sleep(delaytime)\n",
    "\n",
    "    # 小车停止\n",
    "    def brake(self, leftSpeed, rightSpeed, delaytime):\n",
    "        GPIO.output(IN1, GPIO.LOW)\n",
    "        GPIO.output(IN2, GPIO.LOW)\n",
    "        GPIO.output(IN3, GPIO.LOW)\n",
    "        GPIO.output(IN4, GPIO.LOW)\n",
    "        self.pwm_ENA.ChangeDutyCycle(leftSpeed)\n",
    "        self.pwm_ENB.ChangeDutyCycle(rightSpeed)\n",
    "        time.sleep(delaytime)\n",
    "\n",
    "    def stop(self):\n",
    "        self.pwm_ENA.stop()\n",
    "        self.pwm_ENB.stop()\n",
    "    def light(self):\n",
    "        GPIO.output(LED_R, GPIO.HIGH)\n",
    "        GPIO.output(LED_G, GPIO.LOW)\n",
    "        GPIO.output(LED_B, GPIO.LOW)\n",
    "        time.sleep(1)\n",
    "        GPIO.output(LED_R, GPIO.LOW)\n",
    "        GPIO.output(LED_G, GPIO.HIGH)\n",
    "        GPIO.output(LED_B, GPIO.LOW)\n",
    "        time.sleep(1)\n",
    "        GPIO.output(LED_R, GPIO.LOW)\n",
    "        GPIO.output(LED_G, GPIO.LOW)\n",
    "        GPIO.output(LED_B, GPIO.HIGH)\n",
    "    def lightt(self):\n",
    "        GPIO.output(LED_R, GPIO.HIGH)\n",
    "        GPIO.output(LED_G, GPIO.LOW)\n",
    "        GPIO.output(LED_B, GPIO.LOW)\n",
    "        time.sleep(1)\n",
    "        GPIO.output(LED_R, GPIO.LOW)\n",
    "        GPIO.output(LED_G, GPIO.HIGH)\n",
    "        GPIO.output(LED_B, GPIO.LOW)\n",
    "        time.sleep(1)\n",
    "        GPIO.output(LED_R, GPIO.LOW)\n",
    "        GPIO.output(LED_G, GPIO.LOW)\n",
    "        GPIO.output(LED_B, GPIO.HIGH)\n",
    "        time.sleep(1)\n",
    "        GPIO.output(LED_R, GPIO.HIGH)\n",
    "        GPIO.output(LED_G, GPIO.HIGH)\n",
    "        GPIO.output(LED_B, GPIO.LOW)\n",
    "        time.sleep(1)\n",
    "        GPIO.output(LED_R, GPIO.HIGH)\n",
    "        GPIO.output(LED_G, GPIO.LOW)\n",
    "        GPIO.output(LED_B, GPIO.HIGH)\n",
    "        time.sleep(1)\n",
    "        GPIO.output(LED_R, GPIO.LOW)\n",
    "        GPIO.output(LED_G, GPIO.HIGH)\n",
    "        GPIO.output(LED_B, GPIO.HIGH)\n",
    "        time.sleep(1)\n",
    "        GPIO.output(LED_R, GPIO.LOW)\n",
    "        GPIO.output(LED_G, GPIO.LOW)\n",
    "        GPIO.output(LED_B, GPIO.LOW)\n",
    "    def whistle(self):   \n",
    "        GPIO.output(buzzer, GPIO.LOW)\n",
    "        time.sleep(0.1)\n",
    "        GPIO.output(buzzer, GPIO.HIGH)\n",
    "        time.sleep(0.001)\n",
    "print(0)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "1\n"
     ]
    }
   ],
   "source": [
    "#  Ultrasonic ranging module\n",
    "\n",
    "import RPi.GPIO as GPIO\n",
    "import time\n",
    "\n",
    "GPIO.setwarnings(False)\n",
    "GPIO.setmode(GPIO.BCM)\n",
    "\n",
    "\n",
    "class CarUltrasound(object):\n",
    "    def __init__(self):\n",
    "\n",
    "        # 超声波引脚定义\n",
    "        self.GPIO_TRIGGER = 1  # GPIO setting (BCM coding)\n",
    "        self.GPIO_ECHO = 0\n",
    "\n",
    "        GPIO.setup(self.GPIO_TRIGGER, GPIO.OUT)  # GPIO input/output definiation\n",
    "        GPIO.setup(self.GPIO_ECHO, GPIO.IN)\n",
    "\n",
    "        self.dist_mov_ave = 0\n",
    "\n",
    "    def distMeasure(self):  # distance measuing\n",
    "\n",
    "        GPIO.output(self.GPIO_TRIGGER, GPIO.HIGH)\n",
    "        time.sleep(0.000015)\n",
    "        GPIO.output(self.GPIO_TRIGGER, GPIO.LOW)\n",
    "        while not GPIO.input(self.GPIO_ECHO):\n",
    "            pass\n",
    "        t1 = time.time()\n",
    "        while GPIO.input(self.GPIO_ECHO):\n",
    "            pass\n",
    "        t2 = time.time()\n",
    "        print\n",
    "        \"distance is %d \" % (((t2 - t1) * 340 / 2) * 100)\n",
    "        time.sleep(0.01)\n",
    "        return ((t2 - t1) * 340 / 2) * 100\n",
    "\n",
    "        # GPIO.output(self.GPIO_TRIGGER, False)\n",
    "        # time.sleep(0.000002)\n",
    "        # GPIO.output(self.GPIO_TRIGGER, True)  # emit ultrasonic pulse\n",
    "        # time.sleep(0.00001)                   # last 10us\n",
    "        # GPIO.output(self.GPIO_TRIGGER, False) # end the pulse\n",
    "\n",
    "        # ii = 0\n",
    "        # while GPIO.input(self.GPIO_ECHO) == 0:  # when receiving the echo, ECHO will become 1\n",
    "        #     ii = ii + 1\n",
    "        #     if ii > 10000:\n",
    "        #         print('Ultrasound error: the sensor missed the echo')\n",
    "        #         return 0\n",
    "        #     pass\n",
    "        # start_time = time.time()\n",
    "        #\n",
    "        # while GPIO.input(self.GPIO_ECHO) == 1:  # the duration of high level of ECHO is the time between the emitting the pulse and receiving the echo\n",
    "        #         pass\n",
    "        # stop_time = time.time()\n",
    "        #\n",
    "        # time_elapsed = stop_time - start_time\n",
    "        # distance = (time_elapsed * 34300) / 2\n",
    "        #\n",
    "        # return distance\n",
    "\n",
    "    def distMeasureMovingAverage(self):\n",
    "        dist_current = self.distMeasure()\n",
    "        if dist_current == 0:  # if the sensor missed the echo, the output dis_mov_ave will equal the last dis_mov_ave\n",
    "            return self.dist_mov_ave\n",
    "        else:\n",
    "            self.dist_mov_ave = 0.8 * dist_current + 0.2 * self.dist_mov_ave  # using the moving average of distance measured by sensor to reduce the error\n",
    "            return self.dist_mov_ave\n",
    "print(1)\n",
    " "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "2\n"
     ]
    }
   ],
   "source": [
    "\n",
    "import cv2\n",
    "import numpy as np\n",
    "import copy\n",
    "import time\n",
    "\n",
    "\n",
    "class LaneDetect(object):\n",
    "\n",
    "    def __init__(self):\n",
    "        self.VideoReturn = True\n",
    "        self.num_lane_point = 4  # the number of detected points on the lane\n",
    "        self.turn_right_speed = 50\n",
    "        self.turn_left_speed = 50\n",
    "        self.forward_speed = 40\n",
    "        self.speed_high = 60\n",
    "        self.speed_low = 0\n",
    "        self.cap = cv2.VideoCapture(0)\n",
    "\n",
    "    def stop(self):\n",
    "        self.cap.release()\n",
    "        cv2.destroyAllWindows()\n",
    "\n",
    "    def directionDetect(self, imageFrame):\n",
    "        ForB = 'Forward'  # forward or back\n",
    "        LorR = 'Brake'  # return\n",
    "\n",
    "\n",
    "        # 转化为灰度图\n",
    "        gray = cv2.cvtColor(imageFrame, cv2.COLOR_BGR2GRAY)\n",
    "        # cv2.imshow(\"gray_img\", gray)\n",
    "        # 大津法二值化\n",
    "        retval, dst = cv2.threshold(gray, 0, 255, cv2.THRESH_OTSU)\n",
    "\n",
    "        ################## lane detection ##############################################\n",
    "        # img = cv2.blur(imageFrame, (5, 5))  # denoising\n",
    "        # _, _, red_img = cv2.split(\n",
    "        #     img)  # extract the red channel of the RGB image (since the lane in our experiment is blue or black)\n",
    "        # # gray_img = cv2.cvtColor(img, cv2.COLOR_RGB2GRAY)  # rgb to gray\n",
    "        #\n",
    "        # _, dst = cv2.threshold(red_img, 20, 255,\n",
    "        #                        cv2.THRESH_BINARY)  # binaryzation, the thresold deponds on the light in the environment\n",
    "\n",
    "        height, width = dst.shape\n",
    "        half_width = int(width / 2)\n",
    "\n",
    "        right_line_pos = np.zeros((self.num_lane_point, 1))\n",
    "        left_line_pos = np.zeros((self.num_lane_point, 1))\n",
    "\n",
    "        img_out = cv2.cvtColor(dst, cv2.COLOR_GRAY2RGB)\n",
    "        for i in range(self.num_lane_point):  # each detected point on the lane\n",
    "            detect_height = height - 25 * (i + 1)\n",
    "            detect_area_left = dst[detect_height,\n",
    "                               0: half_width - 1]  # divide the image into two parts: left and right (this may cause problems, which can be optimized in the future)\n",
    "            detect_area_right = dst[detect_height, half_width: width - 1]\n",
    "            line_left = np.where(detect_area_left == 0)  # extract  zero pixels' index\n",
    "            line_right = np.where(detect_area_right == 0)\n",
    "\n",
    "            if len(line_left[0]):\n",
    "                left_line_pos[i] = int(np.max(line_left))  # set the most internal pixel as the lane point\n",
    "            else:\n",
    "                left_line_pos[i] = 0  # if haven't detected any zero pixel, set the lane point as 0\n",
    "\n",
    "            if len(line_right[0]):\n",
    "                right_line_pos[i] = int(np.min(line_right))\n",
    "            else:\n",
    "                right_line_pos[i] = half_width - 1\n",
    "\n",
    "            if left_line_pos[i] != 0:  # draw the lane points on the binary image\n",
    "                img_out = cv2.circle(img_out, (left_line_pos[i], detect_height), 4, (0, 190, 255), thickness=10)\n",
    "            if right_line_pos[i] != half_width - 1:\n",
    "                img_out = cv2.circle(img_out, (half_width + right_line_pos[i], detect_height), 4, (0, 0, 255),\n",
    "                                     thickness=10)\n",
    "\n",
    "        if self.VideoReturn:  # detect the tennis & transmit the frames to PC\n",
    "            # car.VideoTransmission(img_out)\n",
    "            cv2.imshow(\"frame\", img_out)\n",
    "#             cv2.imwrite(\"f:\\\\\" + imageFile + \"-out-2.jpg\", img_out)\n",
    "\n",
    "        ############################ decision making #####################################\n",
    "        left_max = np.max(left_line_pos)\n",
    "        right_min = np.min(right_line_pos)  # choose the most internal lane point for decision making\n",
    "\n",
    "        # if no detected lane, then keep the last action\n",
    "        #  if only detected the right lane:\n",
    "        #  if the right lane is still close to the image border, then go straight;\n",
    "        #  if the right lane is too close to the image center, then spin around;\n",
    "        #  else, then turn and go straight\n",
    "        # if only detected the left lane: similar to the above\n",
    "        # if both lanes is detected: go straight\n",
    "        if left_max == 0 and right_min == half_width - 1:\n",
    "            pass\n",
    "        elif left_max == 0:\n",
    "            if right_min > half_width - 100:\n",
    "                ForB = 'Forward'\n",
    "                LorR = 'Brake'\n",
    "            elif right_min < 100:\n",
    "                ForB = 'Brake'\n",
    "                LorR = 'Left'\n",
    "            else:\n",
    "                ForB = 'Forward'\n",
    "                LorR = 'Left'\n",
    "        elif right_min == half_width - 1:\n",
    "            if left_max < 100:\n",
    "                ForB = 'Forward'\n",
    "                LorR = 'Brake'\n",
    "            elif left_max > half_width - 100:\n",
    "                ForB = 'Brake'\n",
    "                LorR = 'Right'\n",
    "            else:\n",
    "                ForB = 'Forward'\n",
    "                LorR = 'Right'\n",
    "        else:\n",
    "            # both lane exist\n",
    "            # 取有效点较多的一条边\n",
    "            left_line_pos = [ele for ele in left_line_pos if ele !=0 ]\n",
    "            right_line_pos = [ele for ele in right_line_pos if ele !=0 ]\n",
    "            line_pos = right_line_pos\n",
    "            if len(left_line_pos) > len(right_line_pos):\n",
    "                line_pos = left_line_pos\n",
    "\n",
    "            maxIndex = len(line_pos) - 1\n",
    "\n",
    "            if ((line_pos[maxIndex] - line_pos[0]) > 20):\n",
    "                ForB = 'Forward'\n",
    "                LorR = 'Right'\n",
    "            elif ((line_pos[0] - line_pos[maxIndex]) > 20):\n",
    "                ForB = 'Forward'\n",
    "                LorR = 'Left'\n",
    "            elif (line_pos[maxIndex] == line_pos[0]):\n",
    "                ForB = 'Forward'\n",
    "                LorR = 'Brake'\n",
    "\n",
    "        ############################ motion control #####################################\n",
    "        carDirection = ''\n",
    "        if ForB is 'Brake':\n",
    "            if LorR is 'Left':\n",
    "                # car.left(turn_left_speed)\n",
    "                carDirection = \"left\"\n",
    "            elif LorR is 'Right':\n",
    "                # car.right(turn_right_speed)\n",
    "                carDirection = \"right\"\n",
    "            elif LorR is 'Brake':\n",
    "                # car.brake()\n",
    "                carDirection = \"brake\"\n",
    "        elif ForB is 'Forward':\n",
    "            if LorR is 'Left':\n",
    "                # car.forward_turn(speed_low, speed_high)\n",
    "                carDirection = \"forward left\"\n",
    "            elif LorR is 'Right':\n",
    "                # car.forward_turn(speed_high, speed_low)\n",
    "                carDirection = \"forward right\"\n",
    "            elif LorR is 'Brake':\n",
    "                # car.forward(forward_speed)\n",
    "                carDirection = \"forward\"\n",
    "        elif ForB is 'Backward':\n",
    "            if LorR is 'Left':\n",
    "                # car.left(turn_left_speed)\n",
    "                carDirection = \"left\"\n",
    "            elif LorR is 'Right':\n",
    "                # car.right(turn_right_speed)\n",
    "                carDirection = \"right\"\n",
    "            elif LorR is 'Brake':\n",
    "                # car.back(40)\n",
    "                carDirection = \"brake\"\n",
    "        print(carDirection)\n",
    "\n",
    "        return carDirection\n",
    "\n",
    "print(2)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "CarMove\n",
      "CarUltrasound\n",
      "LaneDetect\n",
      "start\n",
      "forward\n",
      "forward\n",
      "ultra sound...\n",
      "Distance 61.21826171875\n",
      "start\n",
      "forward left\n",
      "forward left\n",
      "ultra sound...\n",
      "Distance 73.8931655883789\n",
      "start\n",
      "forward\n",
      "forward\n",
      "ultra sound...\n",
      "Distance 76.2530517578125\n",
      "start\n",
      "forward\n",
      "forward\n",
      "ultra sound...\n",
      "Distance 76.6407241821289\n",
      "start\n",
      "forward right\n",
      "forward right\n",
      "ultra sound...\n",
      "Distance 64.23141937255859\n",
      "Measurement stopped by User\n"
     ]
    },
    {
     "ename": "TypeError",
     "evalue": "stop() missing 1 required positional argument: 'self'",
     "output_type": "error",
     "traceback": [
      "\u001b[0;31m---------------------------------------------------------------------------\u001b[0m",
      "\u001b[0;31mKeyboardInterrupt\u001b[0m                         Traceback (most recent call last)",
      "\u001b[0;32m<ipython-input-4-ad1f2e386a68>\u001b[0m in \u001b[0;36m<module>\u001b[0;34m()\u001b[0m\n\u001b[1;32m     94\u001b[0m                     \u001b[0;32melse\u001b[0m\u001b[0;34m:\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[0;32m---> 95\u001b[0;31m                         \u001b[0mcar\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mforward\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0;36m10\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0;36m10\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0;36m1\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[0m\u001b[1;32m     96\u001b[0m                 \u001b[0;32melif\u001b[0m \u001b[0mdist\u001b[0m \u001b[0;34m<\u001b[0m \u001b[0;36m30\u001b[0m\u001b[0;34m:\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n",
      "\u001b[0;32m<ipython-input-1-05340f87594e>\u001b[0m in \u001b[0;36mforward\u001b[0;34m(self, leftSpeed, rightSpeed, delaytime)\u001b[0m\n\u001b[1;32m     70\u001b[0m         \u001b[0mself\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mpwm_ENB\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mChangeDutyCycle\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0mrightSpeed\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[0;32m---> 71\u001b[0;31m         \u001b[0mtime\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0msleep\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0mdelaytime\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[0m\u001b[1;32m     72\u001b[0m \u001b[0;34m\u001b[0m\u001b[0m\n",
      "\u001b[0;31mKeyboardInterrupt\u001b[0m: ",
      "\nDuring handling of the above exception, another exception occurred:\n",
      "\u001b[0;31mTypeError\u001b[0m                                 Traceback (most recent call last)",
      "\u001b[0;32m<ipython-input-4-ad1f2e386a68>\u001b[0m in \u001b[0;36m<module>\u001b[0;34m()\u001b[0m\n\u001b[1;32m    104\u001b[0m     \u001b[0;32mexcept\u001b[0m \u001b[0mKeyboardInterrupt\u001b[0m\u001b[0;34m:\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[1;32m    105\u001b[0m         \u001b[0mprint\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0;34m\"Measurement stopped by User\"\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[0;32m--> 106\u001b[0;31m         \u001b[0mcar\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mallStop\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[0m\u001b[1;32m    107\u001b[0m \u001b[0mprint\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0;36m3\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n",
      "\u001b[0;32m<ipython-input-4-ad1f2e386a68>\u001b[0m in \u001b[0;36mallStop\u001b[0;34m(self)\u001b[0m\n\u001b[1;32m     26\u001b[0m         \u001b[0mCarMove\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mstop\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0mself\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[1;32m     27\u001b[0m         \u001b[0mGPIO\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mcleanup\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[0;32m---> 28\u001b[0;31m         \u001b[0mLaneDetect\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mstop\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[0m\u001b[1;32m     29\u001b[0m     \u001b[0;32mdef\u001b[0m \u001b[0mhongld\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0mself\u001b[0m\u001b[0;34m,\u001b[0m\u001b[0mimage\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m:\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[1;32m     30\u001b[0m         \u001b[0mred1\u001b[0m \u001b[0;34m=\u001b[0m \u001b[0mnp\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0marray\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0;34m[\u001b[0m\u001b[0;36m35\u001b[0m\u001b[0;34m,\u001b[0m\u001b[0;36m43\u001b[0m\u001b[0;34m,\u001b[0m\u001b[0;36m46\u001b[0m\u001b[0;34m]\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n",
      "\u001b[0;31mTypeError\u001b[0m: stop() missing 1 required positional argument: 'self'"
     ]
    }
   ],
   "source": [
    "\n",
    "# move, ultrasound, color recognition\n",
    "\n",
    "import RPi.GPIO as GPIO\n",
    "import time\n",
    "import numpy as np\n",
    "import cv2\n",
    "\n",
    "# from CarMove import CarMove\n",
    "# from UltraSound import CarUltrasound\n",
    "# from laneDetect import LaneDetect\n",
    "\n",
    "GPIO.setwarnings(False)  # Disable warning\n",
    "GPIO.setmode(GPIO.BCM)  # BCM coding \n",
    "\n",
    "class CarV4(CarMove, CarUltrasound, LaneDetect):  # create class Car, which derives all the modules\n",
    "    def __init__(self):\n",
    "        CarMove.__init__(self)\n",
    "        print(\"CarMove\")\n",
    "        CarUltrasound.__init__(self)\n",
    "        print(\"CarUltrasound\")\n",
    "        LaneDetect.__init__(self)\n",
    "        print(\"LaneDetect\")\n",
    "\n",
    "    def allStop(self):\n",
    "        CarMove.stop(self)\n",
    "        GPIO.cleanup()\n",
    "        LaneDetect.stop()\n",
    "    def hongld(self,image):\n",
    "        red1 = np.array([35,43,46])\n",
    "        red2 = np.array([77,255,255])\n",
    "        img=cv2.cvtColor(image,cv2.COLOR_BGR2GRAY)\n",
    "        hsv=cv2.cvtColor(image,cv2.COLOR_BGR2HSV)\n",
    "        mask=cv2.inRange(hsv,red1,red2)\n",
    "        #h,w=mask.shape\n",
    "        #mask=np.reshape(mask,(h,w,3))\n",
    "        #contours, hierarchy = cv2.findContours(img, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE)\n",
    "        con=cv2.HoughCircles(mask,cv2.HOUGH_GRADIENT,1,20,param1=100,param2=30,minRadius=10,maxRadius=100)\n",
    "        if con is not None:\n",
    "            b = 0\n",
    "            for i in con[0,:]:\n",
    "                c = (int(mask[int(i[1])][int(i[0])]) + int(mask[int(i[1])][int(i[0] + 1)]) + int(mask[int(i[1] + 1)][int(i[0])]) + int(mask[int(i[1])][int(i[0] - 1)]) + int(mask[int(i[1] - 1)][int(i[0])])) / 5\n",
    "                if (c > 0):\n",
    "                    b = 1\n",
    "                # cv2.circle(image,(i[0],i[1]),i[2],(0,255,0),2)\n",
    "            # cv2.imshow(\"input\", image)\n",
    "            # cv2.waitKey(0)\n",
    "            if (b == 1):\n",
    "                return 0\n",
    "            else:\n",
    "                return 1\n",
    "\n",
    "if __name__ == '__main__':\n",
    "    try:\n",
    "        car = CarV4()\n",
    "        while True:\n",
    "            print(\"start\")\n",
    "            r,img = car.cap.read()\n",
    "            b=car.hongld(img)\n",
    "            if b==0:\n",
    "                car.lightt()\n",
    "                car.brake(0,0,1)\n",
    "                sleep(1)\n",
    "            else:\n",
    "                dist = car.distMeasure()\n",
    "                if dist >= 30:\n",
    "                    #寻迹\n",
    "                    # color detect\n",
    "                    ret, frame = car.cap.read()\n",
    "        #             frame_origin = cv2.imread('predict/' + imageFile + '.png')\n",
    "                    sp = frame.shape  # 获取图像形状：返回【行数值，列数值】列表\n",
    "                    sz1 = sp[0]  # 图像的高度（行 范围）\n",
    "                    sz2 = sp[1]  # 图像的宽度（列 范围）\n",
    "                    # sz3 = sp[2]                #像素值由【RGB】三原色组成\n",
    "\n",
    "                    # 裁剪图像, 因为摄像头视角与目标路线区域之间存在视线距离，在这里截取出最前端的一部份图像。\n",
    "                    # 这与开车时人的前方视角被车前盖遮挡的原理类似，小车前端最近的图像有部分是无法准确覆盖，所以取最前端的路线图像，并进行被覆盖区域的距离长度计算。\n",
    "                    cropImg = frame[30:(sz1 - 150), 0:sz2]\n",
    "                    carDirection = car.directionDetect(cropImg)\n",
    "        #           carDirection = car.directionDetect(frame)\n",
    "\n",
    "                    print(carDirection)\n",
    "                    if \"brake\" in carDirection:\n",
    "                        car.brake(0, 0, 1)\n",
    "                        print(\"stop\")\n",
    "                    if \"forward\" in carDirection:\n",
    "                        car.forward(35, 35, 1)\n",
    "\n",
    "                    if \"right\" in carDirection:\n",
    "                        car.right(20, 20, 1)\n",
    "                    if \"left\" in carDirection:\n",
    "                        car.forward(25, 25, 1)\n",
    "                        car.left(20, 20, 2)\n",
    "                        print(left)\n",
    "\n",
    "                    print(\"ultra sound...\")\n",
    "                    # perception\n",
    "                    dist_mov_ave = car.distMeasureMovingAverage()\n",
    "                    print('Distance', dist_mov_ave)\n",
    "                    # decison-making\n",
    "                    if dist_mov_ave < 20:\n",
    "                        car.brake(0, 0, 10)\n",
    "                    else:\n",
    "                        car.forward(10, 10, 1)\n",
    "                elif dist < 30:\n",
    "                    car.whistle()\n",
    "                    car.light()\n",
    "                    car.left(30,30,1)\n",
    "                    car.right(30,30,1)\n",
    "                    car.run(10,10,1)\n",
    "                    time.sleep(1)\n",
    "\n",
    "    except KeyboardInterrupt:\n",
    "        print(\"Measurement stopped by User\")\n",
    "        car.allStop()\n",
    "print(3)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "\n",
    "import cv2\n",
    "import numpy as np\n",
    "import copy\n",
    "import time\n",
    "\n",
    "\n",
    "class LaneDetect(object):\n",
    "\n",
    "    def __init__(self):\n",
    "        self.VideoReturn = True\n",
    "        self.num_lane_point = 4  # the number of detected points on the lane\n",
    "        self.turn_right_speed = 50\n",
    "        self.turn_left_speed = 50\n",
    "        self.forward_speed = 40\n",
    "        self.speed_high = 60\n",
    "        self.speed_low = 0\n",
    "        self.cap = cv2.VideoCapture(0)\n",
    "\n",
    "    def stop(self):\n",
    "        self.cap.release()\n",
    "        cv2.destroyAllWindows()\n",
    "\n",
    "    def directionDetect(self, imageFrame):\n",
    "        ForB = 'Forward'  # forward or back\n",
    "        LorR = 'Brake'  # return\n",
    "\n",
    "\n",
    "        # 转化为灰度图\n",
    "        gray = cv2.cvtColor(imageFrame, cv2.COLOR_BGR2GRAY)\n",
    "        # cv2.imshow(\"gray_img\", gray)\n",
    "        # 大津法二值化\n",
    "        retval, dst = cv2.threshold(gray, 0, 255, cv2.THRESH_OTSU)\n",
    "\n",
    "        ################## lane detection ##############################################\n",
    "        # img = cv2.blur(imageFrame, (5, 5))  # denoising\n",
    "        # _, _, red_img = cv2.split(\n",
    "        #     img)  # extract the red channel of the RGB image (since the lane in our experiment is blue or black)\n",
    "        # # gray_img = cv2.cvtColor(img, cv2.COLOR_RGB2GRAY)  # rgb to gray\n",
    "        #\n",
    "        # _, dst = cv2.threshold(red_img, 20, 255,\n",
    "        #                        cv2.THRESH_BINARY)  # binaryzation, the thresold deponds on the light in the environment\n",
    "\n",
    "        height, width = dst.shape\n",
    "        half_width = int(width / 2)\n",
    "\n",
    "        right_line_pos = np.zeros((self.num_lane_point, 1))\n",
    "        left_line_pos = np.zeros((self.num_lane_point, 1))\n",
    "\n",
    "        img_out = cv2.cvtColor(dst, cv2.COLOR_GRAY2RGB)\n",
    "        for i in range(self.num_lane_point):  # each detected point on the lane\n",
    "            detect_height = height - 25 * (i + 1)\n",
    "            detect_area_left = dst[detect_height,\n",
    "                               0: half_width - 1]  # divide the image into two parts: left and right (this may cause problems, which can be optimized in the future)\n",
    "            detect_area_right = dst[detect_height, half_width: width - 1]\n",
    "            line_left = np.where(detect_area_left == 0)  # extract  zero pixels' index\n",
    "            line_right = np.where(detect_area_right == 0)\n",
    "\n",
    "            if len(line_left[0]):\n",
    "                left_line_pos[i] = int(np.max(line_left))  # set the most internal pixel as the lane point\n",
    "            else:\n",
    "                left_line_pos[i] = 0  # if haven't detected any zero pixel, set the lane point as 0\n",
    "\n",
    "            if len(line_right[0]):\n",
    "                right_line_pos[i] = int(np.min(line_right))\n",
    "            else:\n",
    "                right_line_pos[i] = half_width - 1\n",
    "\n",
    "            if left_line_pos[i] != 0:  # draw the lane points on the binary image\n",
    "                img_out = cv2.circle(img_out, (left_line_pos[i], detect_height), 4, (0, 190, 255), thickness=10)\n",
    "            if right_line_pos[i] != half_width - 1:\n",
    "                img_out = cv2.circle(img_out, (half_width + right_line_pos[i], detect_height), 4, (0, 0, 255),\n",
    "                                     thickness=10)\n",
    "\n",
    "        if self.VideoReturn:  # detect the tennis & transmit the frames to PC\n",
    "            # car.VideoTransmission(img_out)\n",
    "            cv2.imshow(\"frame\", img_out)\n",
    "#             cv2.imwrite(\"f:\\\\\" + imageFile + \"-out-2.jpg\", img_out)\n",
    "\n",
    "        ############################ decision making #####################################\n",
    "        left_max = np.max(left_line_pos)\n",
    "        right_min = np.min(right_line_pos)  # choose the most internal lane point for decision making\n",
    "\n",
    "        # if no detected lane, then keep the last action\n",
    "        #  if only detected the right lane:\n",
    "        #  if the right lane is still close to the image border, then go straight;\n",
    "        #  if the right lane is too close to the image center, then spin around;\n",
    "        #  else, then turn and go straight\n",
    "        # if only detected the left lane: similar to the above\n",
    "        # if both lanes is detected: go straight\n",
    "        if left_max == 0 and right_min == half_width - 1:\n",
    "            pass\n",
    "        elif left_max == 0:\n",
    "            if right_min > half_width - 100:\n",
    "                ForB = 'Forward'\n",
    "                LorR = 'Brake'\n",
    "            elif right_min < 100:\n",
    "                ForB = 'Brake'\n",
    "                LorR = 'Left'\n",
    "            else:\n",
    "                ForB = 'Forward'\n",
    "                LorR = 'Left'\n",
    "        elif right_min == half_width - 1:\n",
    "            if left_max < 100:\n",
    "                ForB = 'Forward'\n",
    "                LorR = 'Brake'\n",
    "            elif left_max > half_width - 100:\n",
    "                ForB = 'Brake'\n",
    "                LorR = 'Right'\n",
    "            else:\n",
    "                ForB = 'Forward'\n",
    "                LorR = 'Right'\n",
    "        else:\n",
    "            # both lane exist\n",
    "            # 取有效点较多的一条边\n",
    "            left_line_pos = [ele for ele in left_line_pos if ele !=0 ]\n",
    "            right_line_pos = [ele for ele in right_line_pos if ele !=0 ]\n",
    "            line_pos = right_line_pos\n",
    "            if len(left_line_pos) > len(right_line_pos):\n",
    "                line_pos = left_line_pos\n",
    "\n",
    "            maxIndex = len(line_pos) - 1\n",
    "\n",
    "            if ((line_pos[maxIndex] - line_pos[0]) > 20):\n",
    "                ForB = 'Forward'\n",
    "                LorR = 'Right'\n",
    "            elif ((line_pos[0] - line_pos[maxIndex]) > 20):\n",
    "                ForB = 'Forward'\n",
    "                LorR = 'Left'\n",
    "            elif (line_pos[maxIndex] == line_pos[0]):\n",
    "                ForB = 'Forward'\n",
    "                LorR = 'Brake'\n",
    "\n",
    "        ############################ motion control #####################################\n",
    "        carDirection = ''\n",
    "        if ForB is 'Brake':\n",
    "            if LorR is 'Left':\n",
    "                # car.left(turn_left_speed)\n",
    "                carDirection = \"left\"\n",
    "            elif LorR is 'Right':\n",
    "                # car.right(turn_right_speed)\n",
    "                carDirection = \"right\"\n",
    "            elif LorR is 'Brake':\n",
    "                # car.brake()\n",
    "                carDirection = \"brake\"\n",
    "        elif ForB is 'Forward':\n",
    "            if LorR is 'Left':\n",
    "                # car.forward_turn(speed_low, speed_high)\n",
    "                carDirection = \"forward left\"\n",
    "            elif LorR is 'Right':\n",
    "                # car.forward_turn(speed_high, speed_low)\n",
    "                carDirection = \"forward right\"\n",
    "            elif LorR is 'Brake':\n",
    "                # car.forward(forward_speed)\n",
    "                carDirection = \"forward\"\n",
    "        elif ForB is 'Backward':\n",
    "            if LorR is 'Left':\n",
    "                # car.left(turn_left_speed)\n",
    "                carDirection = \"left\"\n",
    "            elif LorR is 'Right':\n",
    "                # car.right(turn_right_speed)\n",
    "                carDirection = \"right\"\n",
    "            elif LorR is 'Brake':\n",
    "                # car.back(40)\n",
    "                carDirection = \"brake\"\n",
    "\n",
    "        return carDirection\n",
    "    \n",
    "print(4)\n",
    " "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.7.3"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 4
}
